#!/usr/bin/env python3
import rospy
from geometry_msgs.msg import PoseWithCovarianceStamped

def publish_initial_pose():
    rospy.init_node('initial_pose_publisher')
    pub = rospy.Publisher('/initialpose', PoseWithCovarianceStamped, queue_size=1)
    rospy.sleep(5) 

    msg = PoseWithCovarianceStamped()
    msg.header.stamp = rospy.Time.now()
    msg.header.frame_id = "map"
    msg.pose.pose.position.x = 0
    msg.pose.pose.position.y = 0
    msg.pose.pose.orientation.w = 1.0  # yaw=0
    
    # 修正协方差矩阵为36个元素
    msg.pose.covariance = [0.0] * 36
    msg.pose.covariance[0] = 0.25   # X的方差
    msg.pose.covariance[7] = 0.25   # Y的方差
    msg.pose.covariance[35] = 0.0685  # Yaw的方差
    
    pub.publish(msg)
    rospy.loginfo("Initial pose published at (-5, 3, 0)")

if __name__ == "__main__":
    publish_initial_pose()